# these code can recognize face and draw the rectangle.new version
import abc
from locale import ABDAY_1
from traceback import print_tb
import numpy as np
from pygame import PixelArray
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
import argparse
import cv2
import sys
from djitellopy import Tello
import math
import matplotlib.pyplot as plt
import numpy as np
# import pandas as pd
# import cv2.aruco
tello = Tello()

class RAS_Tello(Node):
    def __init__(self):
        super().__init__('tello_project')
        self.subscription = self.create_subscription(
            Image,
            '/image_raw',
            self.image_callback,
            10
        )
        self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
        self.tello = Tello()
    def image_callback(self, msg):
        self.frame = self.imgmsg_to_cv2(msg)   ###-------------------------------------------------
        # upload net
        face_cascade = cv2.CascadeClassifier('Resources/haarcascade_frontalface_default.xml')
        imgGray = cv2.cvtColor(self.frame, cv2.COLOR_BGR2GRAY)  #add gray pic
        faces = face_cascade.detectMultiScale(image = imgGray, scaleFactor = 1.1, minNeighbors = 5)
        myFaceListC = []
        myFaceListArea = []
        size=self.frame.shape
        # print sieze of img and center of img 
        x_center,y_center=int(size[0]/2),int(size[1]/2)
        cv2.circle(self.frame, (y_center,x_center), 5, (0, 0, 255), cv2.FILLED)
        # recoginze face
        faces = face_cascade.detectMultiScale(image = self.frame, scaleFactor = 1.1, minNeighbors = 5)
        # draw a rectangle of face
        # if len(faces)>0:
        for (x, y, w, h) in faces:
            cv2.rectangle(self.frame, (x, y), (x+w, y+h), (255, 0, 0), 2)
            cx = x + w//2
            cy = y + h//2
            area = w*h
            #print the center of face cx cy
            cv2.circle(self.frame, (cx, cy), 5, (255, 0, 0), cv2.FILLED)
            myFaceListC.append([cx,cy])
            myFaceListArea.append(area)
        #judge the the exist of face, and print the rectangle and necessary text
        if len(myFaceListArea) != 0:
            i = myFaceListArea.index(max(myFaceListArea))
            InfoText = "Area:{0} X:{1} Y:{2}".format(area , cx, cy)
            cv2.putText(self.frame, InfoText, (10, 20),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1)
            finalimg = cv2.cvtColor(self.frame, cv2.COLOR_RGBA2RGB)
        else:
            # self.tello_hover()
            finalimg = cv2.cvtColor(self.frame, cv2.COLOR_RGBA2RGB)
        # contral the tello according the (centers & area) of face and pic
        tello_pixel=w*h
        if cx in range(x_center-10,x_center+10) & cy in range(y_center-10, y_center+10):
            self.tello_forward()
        # if cx in range(x_center-10,x_center+10) & cy in range(y_center-10, y_center+10) & area in range(300,800):
        #     self.tello_stand()
        if x_center > cx:
            self.tello_right() 
        if x_center < cx:
            self.tello_left()
        if y_center < cy:
            self.tello_up()
        if y_center > cy:
            self.tello_down()

        cv2.imshow("Frame", finalimg)
        cv2.waitKey(1)
        data=data.append(tello_pixel)
        df = pd.DataFrame(data)
        df.plot.box(title="hua tu")
        plt.grid(linestyle="--", alpha=0.3)
        plt.show()


    def tello_up(self):
        msg=Twist()
        msg.linear.z=0.2
        self.publisher_.publish(msg)
        print('[info] Tello moving up')
    def tello_down(self):
        msg=Twist()
        msg.linear.z=-0.2
        self.publisher_.publish(msg)
        print('[info] Tello moving down')
    def tello_left(self):
        msg=Twist()
        msg.linear.y=-0.1
        self.publisher_.publish(msg)
        print('[info] Tello moving left')
    def tello_right(self):
        msg=Twist()
        msg.linear.y=0.1
        self.publisher_.publish(msg)
        print('[info] Tello moving right')
    def tello_forward(self):
        msg=Twist()
        msg.linear.x=0.3
        self.publisher_.publish(msg)
        print('[info] Tello moving forword')
    def tello_back(self):
        msg=Twist()
        msg.linear.x=-0.2
        self.publisher_.publish(msg)
        print('[info] Tello moving back')
    def tello_hover(self):
        msg = Twist()
        msg.linear.z = 0.1
        msg.angular.z=0.3        
        self.publisher_.publish(msg)
        print('[INFO] Tello up')
    
    # def tello_hover(self):
    #     msg = Twist()
    #     msg.linear.x = 0.001        
    #     self.publisher_.publish(msg)
    #     print('[INFO] Tello is standing')

    def imgmsg_to_cv2(self, img_msg):
        n_channels = len(img_msg.data) // (img_msg.height * img_msg.width)
        dtype = np.uint8

        img_buf = np.asarray(img_msg.data, dtype=dtype) if isinstance(img_msg.data, list) else img_msg.data

        if n_channels == 1:
            cv2_img = np.ndarray(shape=(img_msg.height, img_msg.width), dtype=dtype, buffer=img_buf)
        else:
            cv2_img = np.ndarray(shape=(img_msg.height, img_msg.width, n_channels), dtype=dtype, buffer=img_buf)
        # If the byte order is different between the message and the system
        if img_msg.is_bigendian == (sys.byteorder == 'little'):
            cv2_img = cv2_img.byteswap().newbyteorder()
        return cv2_img

def main(args=None):
    rclpy.init(args=args)
    tello_project = RAS_Tello()
    rclpy.spin(tello_project)
    tello_project.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()